#include "ubt_mqtt/message_manager.h"
#include "ubt_mqtt/config.h"
#include "ubt_mqtt/ubt_utils.h"

namespace ubt
{

static std::string PackMessageData(const Json::Value &data, const char* requestid = nullptr)
{
    Json::Value root;
    Json::StreamWriterBuilder builder;
    std::string time_str = ubt_utils::getTimeStr();

    if (requestid == nullptr)
    {
        root["request_id"] = Json::Value("send_" + time_str);
    }
    else
    {
        root["request_id"] = Json::Value(requestid);
    }
    root["op"]         = Json::Value(ConfigDataInfo::Instance().getRobotName());
    root["sn"]         = Json::Value(ConfigDataInfo::Instance().getRobotSerialNumber());
    root["timestamp"]  = Json::Value(time_str);
    root["data"]       = Json::Value(data);

    builder["indentation"] = ""; // 保证输出的字符串是无格式的
    return Json::writeString(builder, root);
}

CMessageManager::CMessageManager()
{
    _heartbeat_pingtime = 0;
}

CMessageManager::~CMessageManager()
{

}

std::string CMessageManager::PackEventMessageData(const Json::Value &data)
{
    return PackMessageData(data);
}

void CMessageManager::PraseHeartBeatMessage(const char* msg, int len)
{
#if 0
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)msg), root) == false)
    {
        return;
    }
#endif

    _last_recv_heartbeart_time = ubt_utils::getTimeMilliseconds();
    _heartbeat_pingtime = _last_recv_heartbeart_time - _send_heartbeat_time;
}

std::string CMessageManager::PackHeartBeatMessage()
{
    Json::Value data;

    data["cmd"]        = Json::Value("request");
    data["ping"]       = Json::Value(_heartbeat_pingtime);

    _send_heartbeat_time = ubt_utils::getTimeMilliseconds();
    return PackMessageData(data);
}

std::string CMessageManager::PackTopicRequestMessage(const std::string &requestid, bool success, std::string &message)
{
    Json::Value data;

    data["success"]       = Json::Value(success);
    data["message"]       = Json::Value(message);

    return PackMessageData(data, requestid.c_str());
}

std::string CMessageManager::PackAgvStatusMessage(const std::vector<int16_t>& status)
{
    Json::Value data;

    for (auto state : status)
    {
        data.append(state);
    }

    return PackMessageData(data);
}

std::string CMessageManager::PackAgvDiagWarnMessage(const std::string& info)
{
    return PackMessageData(info);
}

std::string CMessageManager::PackStateStatusMessage(const Json::Value& stateJson, int duration, double cpuratio)
{
    Json::Value data;
    Json::Value shelf_code;
    int hw_upcode_state = stateJson["hw_upcode"]["state"].asInt();

    if (hw_upcode_state == 1)
    {
        shelf_code["tag"] = stateJson["hw_upcode"]["tag"];
        shelf_code["pose"] = stateJson["hw_upcode"]["pose"];
    }
    else
    {
        shelf_code["tag"] = Json::Value(0);
        shelf_code["pose"]["x"] = Json::Value(0.0);
        shelf_code["pose"]["y"] = Json::Value(0.0);
        shelf_code["pose"]["theta"] = Json::Value(0.0);
    }
    shelf_code["hw_liftstate"] = stateJson["hw_upcode"]["hw_liftstate"];;

    data["status"]     = stateJson["state"];
    data["duration"]   = Json::Value(duration);
    data["shelf_code"] = shelf_code;
    data["cpu_ratio"]  = Json::Value(cpuratio);

    return PackMessageData(data);
}

std::string CMessageManager::PackTaskStatusMessage(const std::string &requestid, const Json::Value &taskstatus)
{
    return PackMessageData(taskstatus, requestid.c_str());
}

std::string CMessageManager::PackLocalizationMessage(const location_pose_t &realpose, float confidence)
{
    Json::Value data;
    Json::Value coordinate;

    coordinate["x"] = Json::Value(realpose.x);
    coordinate["y"] = Json::Value(realpose.y);
    coordinate["t"] = Json::Value(realpose.theta);

    data["real_coordinate"] = Json::Value(coordinate);
    data["confidence"]      = Json::Value(confidence);

    return PackMessageData(data);
}

std::string CMessageManager::PackBatteryDataMessage(const Json::Value& data)
{

    return PackMessageData(data);
}

std::string CMessageManager::PackWifiDataMessage(const Json::Value& data)
{

    return PackMessageData(data);
}

std::string CMessageManager::PackSpeedMessage(float linearspeed, float angularspeed, float totalodom, float totaltime)
{
    Json::Value data;

    data["speed"]     = Json::Value(linearspeed);
    data["wspeed"]    = Json::Value(angularspeed);
    data["totalodom"] = Json::Value(totalodom);
    data["totaltime"] = Json::Value(totaltime);

    return PackMessageData(data);
}

std::string CMessageManager::PackRestDispatchMessage(const std::string& cmd)
{
    Json::Value data;

    data["cmd"] = Json::Value(cmd);

    return PackMessageData(data);
}

std::string CMessageManager::PackResponseMessage(const std::string &responseid, bool status, std::string errmsg)
{
    Json::Value data;

    data["success"] = Json::Value(status);
    data["message"] = Json::Value(errmsg);

    return PackMessageData(data, responseid.c_str());
}

std::string CMessageManager::PackTaskMessage(const map_origin_info_t& mapinfo, const task_info_t &taskinfo, const std::string& version)
{
    Json::Value data;
    Json::Value map;
    Json::Value task;

    map["map_id"]   = Json::Value(mapinfo.id);
    map["map_name"] = Json::Value(mapinfo.name);
    map["map_code"] = Json::Value(mapinfo.code);

    task["task_id"]   = Json::Value(taskinfo.id);
    task["task_name"] = Json::Value(taskinfo.name);
    task["task_code"] = Json::Value(taskinfo.code);

    data["map"] = Json::Value(map);
    data["task"] = Json::Value(task);
    data["config_version"] = Json::Value(version);

    return PackMessageData(data);
}

std::string CMessageManager::PackRequsetConfigMessage(const std::string &responseid, const Json::Value& config)
{
    return PackMessageData(config, responseid.c_str());
}

std::string CMessageManager::PackUpdateMapMessage(const std::string &responseid, const Json::Value& data)
{
    return PackMessageData(data, responseid.c_str());
}

std::string CMessageManager::PackMapImageMessage(const std::string &base64data, int height, int width, const location_pose_t& maporigin)
{
    Json::Value data;
    Json::Value map_yaml;

    map_yaml["image"] = Json::Value(ConfigDataInfo::Instance().getMapImageFilePath() + "\n");
    map_yaml["resolution"] = Json::Value("0.025\n");
    map_yaml["free_thresh"] = Json::Value("0.196\n");
    map_yaml["occupied_thresh"] = Json::Value("0.65\n");
    map_yaml["negate"] = Json::Value("0\n");
    map_yaml["origin"].append(maporigin.x);
    map_yaml["origin"].append(maporigin.y);
    map_yaml["origin"].append(maporigin.theta);

    data["map"] = "data:image/png;base64," + base64data;
    data["width"] = width;
    data["height"] = height;
    data["yaml"] = map_yaml;

    return PackMessageData(data);
}

std::string CMessageManager::PackDispatchInfoMessage(const std::vector<int16_t> &infos)
{
    Json::Value data;

    for (auto info : infos)
    {
        data.append(info);
    }

    return PackMessageData(data);
}

std::string CMessageManager::PackVersionInfoMessage(const robot_version_info_t &version)
{
    Json::Value data;

    data["ros"] = Json::Value(version.ros);
    data["pb"]  = Json::Value(version.powerboard);
    data["mb"]  = Json::Value(version.motor);

    return PackMessageData(data);
}

std::string CMessageManager::PackLidarPointsDataMessage(const std::vector<location_pose_t> &points)
{
    Json::Value data;

    for (auto point : points)
    {
        Json::Value info;
        char xchar[128] = {0};
        char ychar[128] = {0};

        snprintf(xchar, sizeof(xchar), "%.3f", point.x);
        snprintf(ychar, sizeof(ychar), "%.3f", point.y);
        info.append(xchar);
        info.append(ychar);

        data.append(info);
    }

    return PackMessageData(data);
}

} //namespace ubt


